#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

void OnSDKLLsMessageReceived(const sensor_msgs::LaserScan &msg)
{
	ROS_INFO("SDKELI_LS - Laser Scan message received:");
	ROS_INFO("SDKELI_LS - frame_id: %s", msg.header.frame_id.c_str());
	ROS_INFO("SDKELI_LS - timestamp: %d.%d", msg.header.stamp.sec, msg.header.stamp.nsec);
	ROS_INFO("SDKELI_LS - angle_min: %f", msg.angle_min);
	ROS_INFO("SDKELI_LS - angle_max: %f", msg.angle_max);
	ROS_INFO("SDKELI_LS - angle_increment: %f", msg.angle_increment);
	ROS_INFO("SDKELI_LS - time_increment: %f", msg.time_increment);
	ROS_INFO("SDKELI_LS - scan_time: %f", msg.scan_time);
	ROS_INFO("SDKELI_LS - range_min: %f", msg.range_min);
	ROS_INFO("SDKELI_LS - range_max: %f", msg.range_max);

	/*Dump data...*/
	int dataCount = msg.ranges.size();
	ROS_INFO("SDKELI_LS - Data Count: %d", dataCount);
	for(int index = 0; index < dataCount; index ++)
	{
		if(((index +1) % 60 == 0) && (index != 0))
		{
			printf("\n");
		}

		printf("%.2f ", msg.ranges[index]);
	}

	printf("\n\n");
}

int main(int argc, char **argv)
{
	/*Initialize ROS system and node*/
	ros::init(argc, argv, "sdkeli_ls_demo");
	ros::NodeHandle nhSDKLLsMsg;
	ros::Subscriber subSDLKLLsMsg = nhSDKLLsMsg.subscribe("scan", 100, &OnSDKLLsMessageReceived);

	/*Let ROS take over*/
	ros::spin();
}
